/*
 * message.cpp
 *
 *  Created on: 05.12.2013
 *      Author: Strohmi
 */
#include "Arduino.h"
#include "message.h"

message::~message() {
}

message::message(unsigned char sensorID, MESSAGE_TYPE msgType) {
	payload[HEAD] = ((sensorID & 0x0F) << 4) | (msgType & 0x0F);
	payload[LSB] = 0;
	payload[SB3] = 0;
	payload[SB2] = 0;
	payload[MSB] = 0;
}

message::message(unsigned char sensorID, COM_TYPE comType) {
	payload[HEAD] = ((sensorID & 0x0F) << 4) | (COM & 0x0F);
	payload[LSB] = comType;
	payload[SB3] = 0;
	payload[SB2] = 0;
	payload[MSB] = 0;
}

message::message(unsigned char sensorID, SENSOR_TYPE sensorType, VALUE_TYPE valueType,
		unsigned char length) {
	payload[HEAD] = ((sensorID & 0x0F) << 4) | (SENSOR_DESCRIPTION & 0x0F);
	payload[LSB] = sensorType;
	payload[SB2] = valueType;
	payload[SB3] = length;
	payload[MSB] = 0;
}

message::message(unsigned char* msg, char length){
	memcpy(payload, msg, length);
}

void message::setValue(unsigned char value) {
	payload[LSB] = value;
	payload[SB3] = 0;
	payload[SB2] = 0;
	payload[MSB] = 0;
}

void message::setValue(unsigned int value) {
	payload[LSB] = value & 0xFF;
	payload[SB3] = value >> 8;
	payload[SB2] = 0;
	payload[MSB] = 0;
}

void message::setValue(unsigned long value) {
	payload[LSB] = value & 0xFF;
	payload[SB3] = (value & 0xFF00) >> 8;
	payload[SB2] = (value & 0xFF0000) >> 16;
	payload[MSB] = value >> 24;
}

void message::setValue(char value) {
	payload[LSB] = value;
	payload[SB3] = 0;
	payload[SB2] = 0;
	payload[MSB] = 0;
}
void message::setValue(int value) {
	payload[LSB] = value & 0xFF;
	payload[SB3] = value >> 8;
	payload[SB2] = 0;
	payload[MSB] = 0;
}

void message::setValue(long value) {
	payload[LSB] = value & 0xFF;
	payload[SB3] = (value & 0xFF00) >> 8;
	payload[SB2] = (value & 0xFF0000) >> 16;
	payload[MSB] = value >> 24;
}

void message::setValue(float value) {
	memcpy(payload + 1, &value, sizeof(float));
}

void message::setLength(unsigned char length) {
	payload[SB3] = length;
}

void message::setValueType(VALUE_TYPE valueType) {
	payload[SB2] = valueType;
}

void message::setSensorType(SENSOR_TYPE sensorType) {
	payload[LSB] = sensorType;
}

float message::getFloat() {
	float f;
	memcpy(&f, payload + 1, sizeof(float));
	return f;
}

void message::setAddress(unsigned char addr){
	if (payload[LSB] == PAIRING)
		payload[SB2] = addr;
}

int message::getInt() {
	int i = payload[SB2] << 8 | payload[LSB];
	return i;
}

unsigned int message::getUInt() {
	unsigned int i = payload[SB2] << 8 | payload[LSB];
	return i;
}

int message::getComType() {
	return payload[LSB];
}

int message::getLength() {
	return payload[SB3];
}

int message::getValueType() {
	return payload[SB2];
}

int message::getSensorType() {
	return payload[LSB];
}

int message::getMsgType(){
	return payload[HEAD] & 0X0F;
}

int message::getSensorID(){
	return (payload[HEAD] & 0xF0) >> 4;
}

unsigned int message::getAddress(){
	if(payload[LSB] == PAIRING)
		return payload[SB2];
	else
		return 0;
}

char* message::getComTypeString(){
	/*switch (payload[LSB]) {
		case ACK:
			return "ACK";
		case PAIRING:
			return "PAIRING";
		case REQUEST_VALUE:
			return "REQUEST_VALUE";
		case NACK:
			return "REQUEST_VALUE";
		default:
			return "INVALID";
	}*/ return NULL;
}

char* message::getValueTypeString(){
	/*switch (payload[SB2]) {
		case UINT8:
			return "UINT8";
		case UINT16:
			return "UINT16";
		case UINT32:
			return "UINT32";
		case INT8:
			return "INT8";
		case INT16:
			return "INT16";
		case INT32:
			return "INT32";
		case FLOAT:
			return "FLOAT";
		default:
			return "INVALID";
	}*/return NULL;
}

char* message::getSensorTypeString(){
	/*switch (payload[LSB]) {
		case UNDEFINED:
			return "UNDEFINED";
		case BATTERY:
			return "BATTERY";
		case TEMPRATURE:
			return "TEMPRATURE";
		case ACCELERATION:
			return "ACCELERATION";
		case GYROSCOPE:
			return "GYROSCOPE";
		case MAGNETOMETER:
			return "MAGNETOMETER";
		case IMU:
			return "IMU";
		case PRESSURE:
			return "PRESSURE";
		case ENCODER:
			return "ENCODER";
		case STRAINGAGES:
			return "STRAINGAGES";
		case BRIGHTNESS:
			return "BRIGHTNESS";
		case AIRPRESSURE:
			return "AIRPRESSURE";
		case VELOCITY:
			return "VELOCITY";
		case POSITION:
			return "POSITION";
		case ANGLE:
				return "ANGLE";
		default:
			return "INVALID";
	}*/return NULL;
}

char* message::getMsgTypeString(){
	switch (payload[HEAD] & 0x0F) {
		case COM:
			return "COM";
		case SENSOR_DESCRIPTION:
			return "SENSOR_DESCRIPTION";
		case MSG_UINT8:
			return "MSG_UINT8";
		case MSG_UINT16:
			return "MSG_UINT16";
		case MSG_UINT32:
			return "MSG_UINT32";
		case MSG_INT8:
			return "MSG_INT8";
		case MSG_INT16:
			return "MSG_INT16";
		case MSG_INT32:
			return "MSG_INT32";
		case MSG_FLOAT:
			return "MSG_FLOAT";
		case ERROR:
			return "ERROR";
		default:
			return "INVALID";
	}
}

unsigned char* message::toCharArray(){
	return payload;
}

void message::print() {
	for(uint8_t i = 0; i < MSG_LENGTH; i++) {
		Serial.print(payload[i], HEX);
		if(i != MSG_LENGTH - 1)
			Serial.print('-');
	}
	Serial.println();
}
